function [ vebr, omegaNBB, omegaNBBRate, tSects ] = gentgi_navtest_regular( firstStatDuration, transDisplace, transVel, transAccel, rotAngRate )
%GENTGI_NAVTEST_REGULAR 生成轨迹生成器输入量-导航算法测试轨迹（规则型）
%
% Input Arguments
% # firstStatDuration: 标量，首个静止位置时间，默认3600，单位s
% # transDisplace: 标量，平移距离，默认10000，单位m
% # transVel: 标量，平移速率，默认100，单位m/s
% # transAccel: 标量，平移加速率，默认10，单位m/s^2
% # rotAngRate: 标量，转动速率，默认pi/20，单位rad/s
%
% Output Arguments
% # vebr, omeganbb, tSects: 参见gentgi_static函数帮助

if nargin < 5
    rotAngRate = pi/20;
end
if nargin < 4
    transAccel = 10;
end
if nargin < 3
    transVel = 100;
end
if nargin < 2
    transDisplace = 10000;
end
if nargin < 1
    firstStatDuration = 1000;
end

%% 轨迹参数
rotAccTime = 1; % 单位：s
rotDecTime = 1; % 单位：s
rotVec = pi/2 * [1 0 0; 0 1 0; 0 0 1; -1 0 0; 0 -1 0; 0 0 -1; 1/sqrt(3) 1/sqrt(3) 1/sqrt(3); -1/sqrt(3) -1/sqrt(3) -1/sqrt(3)]'; % 负号代表逆向转动

transAccTime = 1; % 单位：s
transDecTime = 1; % 单位：s
transUniVec = [1 0 0; 0 1 0; 0 0 1; -1 0 0; 0 -1 0; 0 0 -1; 1/sqrt(3) 1/sqrt(3) 1/sqrt(3); -1/sqrt(3) -1/sqrt(3) -1/sqrt(3)]'; % 负号代表逆向转动

dt = 0.1; % 单位：s

%% 生成并合并各轨迹段
% 静止段
[vebrValues, omegaNBBValues, omegaNBBRateValues, t] = geneletgi_static(0, firstStatDuration, dt);
tSects(1, 1) = 0;
tSects(2, 1) = t(end);
for i=1:length(transUniVec)
    % 平移段
    [sectvebrValues, sectomegaNBBValues, sectomegaNBBRateValues, sectt] = geneletgi_transalgbdfixaxis_targdisp(t(end), transUniVec(:, i), transDisplace, transVel, transAccTime, transDecTime, dt);
    t = vertcat(t, sectt);
    vebrValues = vertcat(vebrValues, sectvebrValues);
    omegaNBBValues = vertcat(omegaNBBValues, sectomegaNBBValues);
    omegaNBBRateValues = vertcat(omegaNBBRateValues, sectomegaNBBRateValues);
    tSects(end+1, 1) = t(end);
    % 转动段
    [sectvebrValues, sectomegaNBBValues, sectomegaNBBRateValues, sectt] = geneletgi_rotabtbdfixaxis(t(end), rotVec(:, i), rotAngRate, rotAccTime, rotDecTime, dt);
    t = vertcat(t, sectt);
    vebrValues = vertcat(vebrValues, sectvebrValues);
    omegaNBBValues = vertcat(omegaNBBValues, sectomegaNBBValues);
    omegaNBBRateValues = vertcat(omegaNBBRateValues, sectomegaNBBRateValues);
    tSects(end+1, 1) = t(end);
end
% 静止段
[sectvebrValues, sectomegaNBBValues, sectomegaNBBRateValues, sectt] = geneletgi_static(t(end), 5, dt);
t = vertcat(t, sectt);
vebrValues = vertcat(vebrValues, sectvebrValues);
omegaNBBValues = vertcat(omegaNBBValues, sectomegaNBBValues);
omegaNBBRateValues = vertcat(omegaNBBRateValues, sectomegaNBBRateValues);
tSects(end+1, 1) = t(end);
% 转动平移段
for i=1:length(rotVec)
    [~, sectomegaNBBValues, sectomegaNBBRateValues, sectt1] = geneletgi_rotabtbdfixaxis(t(end), rotVec(:, i), rotAngRate, rotAccTime, rotDecTime, dt);
    [sectvebrValues, ~, ~, sectt2] = geneletgi_transalgbdfixaxis_targvel(t(end), transUniVec(:, i), transVel, transAccel, transAccTime, transDecTime, dt);
    if length(sectt1) > length(sectt2)
        t = vertcat(t, sectt1);
        vebrValues = vertcat(vebrValues, sectvebrValues, zeros(length(sectt1)-length(sectt2), 3));
        omegaNBBValues = vertcat(omegaNBBValues, sectomegaNBBValues);
        omegaNBBRateValues = vertcat(omegaNBBRateValues, sectomegaNBBRateValues);
    else
        t = vertcat(t, sectt2);
        vebrValues = vertcat(vebrValues, sectvebrValues);
        omegaNBBValues = vertcat(omegaNBBValues, sectomegaNBBValues, zeros(length(sectt2)-length(sectt1), 3));
        omegaNBBRateValues = vertcat(omegaNBBRateValues, sectomegaNBBRateValues, zeros(length(sectt2)-length(sectt1), 3));
    end
    tSects(end+1, 1) = t(end);
    
    [sectvebrValues, sectomegaNBBValues, sectomegaNBBRateValues, sectt] = geneletgi_static(t(end), 5, dt);
    t = vertcat(t, sectt);
    vebrValues = vertcat(vebrValues, sectvebrValues);
    omegaNBBValues = vertcat(omegaNBBValues, sectomegaNBBValues);
    omegaNBBRateValues = vertcat(omegaNBBRateValues, sectomegaNBBRateValues);
    tSects(end+1, 1) = t(end);
end

vebr.time = t;
vebr.signals.values = vebrValues;
vebr.signals.dimensions = 3;
omegaNBB.time = t;
omegaNBB.signals.values = omegaNBBValues;
omegaNBB.signals.dimensions = 3;
omegaNBBRate.time = t;
omegaNBBRate.signals.values = omegaNBBRateValues;
omegaNBBRate.signals.dimensions = 3;
end